package edu.tfh.s2.ehkbot.steuerung;

import edu.tfh.s2.ehkbot.EHKBotMain;
import edu.tfh.s2.ehkbot.daten.Einstellungen;
import edu.tfh.s2.ehkbot.gerete.Greifarm;
import edu.tfh.s2.ehkbot.gerete.sensoren.Bild;
import edu.tfh.s2.ehkbot.inventar.Regalfach;
import edu.tfh.s2.ehkbot.inventar.Ware;

/**
 * ObjektGreifen versammelt die abläufe die für das Objekt greifen notwendig sind.
 * 
 * @author s2zehn
 */
public class ObjektGreifen {
	/**
	 * Wird geworfen wenn Greifen nicht möglich ist.
	 */
	public static class GreifenNichtMoeglich extends Exception {
		private static final long serialVersionUID = -1004880425120987170L;

		public GreifenNichtMoeglich() {
			super();
		}

		public GreifenNichtMoeglich(String arg0) {
			super(arg0);
		}

		public GreifenNichtMoeglich(String arg0, Throwable arg1) {
			super(arg0, arg1);
		}

		public GreifenNichtMoeglich(Throwable arg0) {
			super(arg0);
		}
	}

	/**
	 * Führt die schritte aus die notwendig sind um eine Ware zu greifen.
	 * 
	 * @param ware
	 *            Die Ware.
	 * @throws GreifenNichtMoeglich
	 *             Wenn Greifen nicht möglich ist.
	 */
	public void greifeWare(Ware ware) throws GreifenNichtMoeglich {
		int greifVersuche = Einstellungen.getInstance().getIntPropertie("GreifVersuche");
		Greifarm greifarm = EHKBotMain.getInstance().getEHKBot().getGreifarm();
		Position pos = null;
		for (int i = 0; i < greifVersuche; ++i) {
			pos = sucheWare(ware);
			if (pos != null)
				break;
		}
		if (pos == null)
			throw new GreifenNichtMoeglich("Ware wurde nicht gefunden");

		greifarm.bewegeArm(pos);
		for (int i = 0; i < greifVersuche; ++i) {
			if (greifarm.getGreifer().greifenMoeglich()) {
				greifarm.getGreifer().greifen();
			} else {
				greifarm.bewegeArm(pos);
			}
			if (greifarm.getGreifer().gegriffen())
				break;
		}
		if (!greifarm.getGreifer().gegriffen())
			throw new GreifenNichtMoeglich("Greifer konnte nicht Greifen");
	}

	/**
	 * Packt die die geladene Warebox in das Regalfach.
	 * 
	 * @param fach
	 */
	public void ladeWarenBoxAb(Regalfach fach) {
	}

	/**
	 * Läd die Warenbox aus den Regal auf.
	 * 
	 * @param fach
	 *            Das Warenbox Regalfach in welchen sich die Warenbox befindet.
	 */
	public void ladeWarenboxAuf(Regalfach fach) {

	}

	/**
	 * Lägt die Ware in die Warenbox.
	 */
	public void legeWareAb() {
		Greifarm greifarm = EHKBotMain.getInstance().getEHKBot().getGreifarm();

		greifarm.bewegeArm(new Position(0, 0, 0));// TODO: Bewegung errechnen

		greifarm.getGreifer().greifenAufheben();

		greifarm.bewegeArm(new Position(0, 0, 0));// TODO: Bewegung errechnen

	}

	/**
	 * Sucht mit den barcode Scanner und der Kamera nach der genauen Position.
	 * 
	 * @param ware
	 *            Die Ware.
	 * @return Die genaue Position.
	 */
	public Position sucheWare(Ware ware) {
		int greifVersuche = Einstellungen.getInstance().getIntPropertie("GreifVersuche");
		Greifarm greifarm = EHKBotMain.getInstance().getEHKBot().getGreifarm();
		Position currentPos = ware.getPosition();
		for (int i = 0; i < greifVersuche; ++i) {
			greifarm.bewegeArm(currentPos);
			int barcode = greifarm.getGreifer().scanBarcode();
			Bild bild = greifarm.getGreifer().getBild();
			double erkannt = 0;
			erkannt += bild.areSameObject(ware.getBild())
							* Einstellungen.getInstance().getDoublePropertie("BildErkanntGewicht");
			if (barcode == ware.getBarcode())
				erkannt += 1.0 * Einstellungen.getInstance().getDoublePropertie("BarcodeErkanntGewicht");
			;

			erkannt = erkannt / 2;

			if (erkannt >= Einstellungen.getInstance().getDoublePropertie("ErkanntGrenzwert")) {
				return currentPos;
			}

			// TODO: Bewegung errechnen

		}
		return null;
	}

}